/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package computing;

import data.ValueMap;
import data.Vector3i;
import com.jme3.math.Vector3f;
import data.Grid3d;
import exception.WaypointsNotFoundException;
import java.util.ArrayList;
import java.util.LinkedList;
import java.util.Queue;
import java.util.Set;
import java.util.logging.Level;
import java.util.logging.Logger;

/**
 *
 * @author oxplay
 */
public class ComputeWaypoints {

    private final static Logger logger = Logger.getLogger(ComputeWaypoints.class.getName());
    private ValueMap valueMap;
    private ArrayList<Vector3f> waypoints = new ArrayList<Vector3f>();
    private Queue<int[]> queue = new LinkedList<int[]>();
    private Grid3d grid;
    private int[] startIntPoint;
    private int[] endIntPoint;
    private int valueMapOffsetX, valueMapOffsetY, valueMapOffsetZ;
    private int valueMapIterationsX, valueMapIterationsY, valueMapIterationsZ;
    private int tmpStartX, tmpStartY, tmpStartZ, tmpEndX, tmpEndY, tmpEndZ;
    private int x, y, z, actualValue;
    private boolean computingDone = false, isDone = false;
    public ComputeWaypoints(Grid3d grid, Vector3i catchStart, Vector3i catchEnd, Vector3i offset) throws WaypointsNotFoundException {
        this.grid = grid;
        valueMap = new ValueMap(catchStart, catchEnd, offset);
        valueMapOffsetX = valueMap.offsetX;
        valueMapOffsetY = valueMap.offsetY;
        valueMapOffsetZ = valueMap.offsetZ;
        valueMapIterationsX = valueMap.iterationsX;
        valueMapIterationsY = valueMap.iterationsY;
        valueMapIterationsZ = valueMap.iterationsZ;
        this.startIntPoint = new int[]{catchStart.x - valueMapOffsetX, catchStart.y - valueMapOffsetY, catchStart.z - valueMapOffsetZ};
        this.endIntPoint = new int[]{catchEnd.x - valueMapOffsetX, catchEnd.y - valueMapOffsetY, catchEnd.z - valueMapOffsetZ};
        logger.setLevel(Level.WARNING);
        logger.log(Level.INFO, "startPoint: {0}", startIntPoint);
        logger.log(Level.INFO, "endPoint: {0}", endIntPoint);
        init();
    }

    public ArrayList<Vector3f> getWaypoints() {
        return waypoints;
    }

    private void init(){
        valueMap.fillValue(-2); // [1]clear area 
        Set<Vector3i> moveableVectors = grid.getMoveableVectors(valueMap.minOffsetPoint, valueMap.maxOffsetPoint);
        logger.log(Level.INFO, "moveableVectors: {0}", moveableVectors);
        valueMap.fillMoveable(moveableVectors);
        valueMap.setValue(startIntPoint[0], startIntPoint[1], startIntPoint[2], 0); // [1]start point value 0
        queue.add(startIntPoint); // [1]add to queue 
    }
    
    public void update(int steps) throws WaypointsNotFoundException{
        while(queue.size() > 0 && (steps <= -1 || steps>0)){
            steps--;
            int[] vect = queue.poll(); // [2]take first and remove
            boolean equalVect = vect[0] == endIntPoint[0] && vect[1] == endIntPoint[1] && vect[2] == endIntPoint[2];
            if (equalVect) {
                computingDone = true;
            } else {
                computeValuesAroundPoint(vect[0], vect[1], vect[2]); // [3]
            }
        }
        if(queue.size() <= 0 || computingDone){
            computingDone = true;
            int endValue = valueMap.getValue(endIntPoint[0], endIntPoint[1], endIntPoint[2]);
            if (endValue == -1) {
                throw new WaypointsNotFoundException("not reached the end point", null);
            }
            buildPath(endIntPoint[0], endIntPoint[1], endIntPoint[2]); // [4]
            isDone = true;
        }
    }
    
    public boolean isDone(){
        return isDone;
    }

    private void fixTmpValues(){
        if (tmpStartX < 0) {
            tmpStartX = 0;
        } else if (tmpStartX >= valueMapIterationsX) {
            tmpStartX = valueMapIterationsX - 1;
        }  
        if (tmpStartY < 0) {
            tmpStartY = 0;
        } else if (tmpStartY >= valueMapIterationsY) {
            tmpStartY = valueMapIterationsY - 1;
        }  
        if (tmpStartZ < 0) {
            tmpStartZ = 0;
        } else if (tmpStartZ >= valueMapIterationsZ) {
            tmpStartZ = valueMapIterationsZ - 1;
        }  
        if (tmpEndX < 0) {
            tmpEndX = 0;
        } else if (tmpEndX >= valueMapIterationsX) {
            tmpEndX = valueMapIterationsX - 1;
        }  
        if (tmpEndY < 0) {
            tmpEndY = 0;
        } else if (tmpEndY >= valueMapIterationsY) {
            tmpEndY = valueMapIterationsY - 1;
        }  
        if (tmpEndZ < 0) {
            tmpEndZ = 0;
        } else if (tmpEndZ >= valueMapIterationsZ) {
            tmpEndZ = valueMapIterationsZ - 1;
        }  
    }
    
    
    private void computeValuesAroundPoint(int vecx, int vecy, int vecz) {
        int centerValue = valueMap.getValue(vecx, vecy, vecz);
        tmpStartX = vecx - 1;
        tmpStartY = vecy - 1;
        tmpStartZ = vecz - 1;
        tmpEndX = vecx + 1;
        tmpEndY = vecy + 1;
        tmpEndZ = vecz + 1;
        fixTmpValues();
        for (x = tmpStartX; x <= tmpEndX; x++) {
            for (z = tmpStartZ; z <= tmpEndZ; z++) {
                for (y = tmpStartY; y <= tmpEndY; y++) {
                    int[] pointVect = new int[]{x, y, z};
                    actualValue = valueMap.getValue(x, y, z);
                    if (actualValue == -1) {
                        valueMap.setValue(x, y, z, centerValue + 1);
                        queue.add(pointVect);
                        logger.log(Level.INFO, "queue add: {0} value: {4} value: {5}", new Object[]{pointVect, centerValue, (centerValue + 1)});
                    }
                }
            }
        }
    }

    private int[] buildPathFindNextVector(int vecx, int vecy, int vecz) {
        int centerValue = valueMap.getValue(vecx, vecy, vecz);
        tmpStartX = vecx - 1;
        tmpStartY = vecy - 1;
        tmpStartZ = vecz - 1;
        tmpEndX = vecx + 1;
        tmpEndY = vecy + 1;
        tmpEndZ = vecz + 1;
        fixTmpValues();
        for (x = tmpStartX; x <= tmpEndX; x++) {
            for (z = tmpStartZ; z <= tmpEndZ; z++) {
                for (y = tmpStartY; y <= tmpEndY; y++) {
                    actualValue = valueMap.getValue(x, y, z);
                    if (actualValue == centerValue - 1) {
                        int[] actVect = new int[]{x, y, z};
                        logger.log(Level.INFO, "buildPathFindNextVectorInner vect: {0} value: {1}", new Object[]{actVect, actualValue});
                        return actVect;
                    }
                }
            }
        }
        return null;
    }

    private void buildPath(int vecx, int vecy, int vecz) throws WaypointsNotFoundException {
        Vector3f waypoint = new Vector3f(Configurator.getIntToFloatValue(valueMapOffsetX + vecx), Configurator.getIntToFloatValue(valueMapOffsetY + vecy), Configurator.getIntToFloatValue(valueMapOffsetZ + vecz));
        waypoints.add(0, waypoint);
        int[] nextVect = buildPathFindNextVector(vecx, vecy, vecz);
        if (nextVect == null) {
            boolean equalVect = vecx == startIntPoint[0] && vecy == startIntPoint[1] && vecz == startIntPoint[2];
            if (equalVect) {
                return;
            } else {
                throw new WaypointsNotFoundException("building path", waypoint);
            }
        } else {
            buildPath(nextVect[0], nextVect[1], nextVect[2]);
        }
    }
}
